#include "ImplicitBuilder.h"
#include <Eigen/Dense>
#include <algorithm>
#include <cmath>
#include <numeric>  // 添加对std::iota的支持
#include <random>   // 添加对随机数的支持
#include <iomanip>  // 添加对std::setprecision的支持
#include <sstream>  // 添加对std::stringstream的支持

std::vector<Point3D> ImplicitBuilder::densifyFaultPillars(
    const std::vector<FaultNode>& faultNodes) {
    
    debug_log("开始加密断层柱线");
    
    std::vector<Point3D> densePoints;
    
    // 按柱线ID分组
    std::unordered_map<int, std::vector<Point3D>> pillars;
    for (const auto& node : faultNodes) {
        pillars[node.column_id].emplace_back(node.x, node.y, node.z, 0);
    }
    
    // 对每条柱线进行加密
    for (const auto& [id, pillarPoints] : pillars) {
        // 按z坐标排序
        auto points = pillarPoints;
        std::sort(points.begin(), points.end(), 
            [](const Point3D& a, const Point3D& b) { return a.z < b.z; });
        
        // 计算点间距离并插值
        for (size_t i = 0; i < points.size() - 1; ++i) {
            const auto& p1 = points[i];
            const auto& p2 = points[i + 1];
            
            double distance = std::sqrt(
                std::pow(p2.x - p1.x, 2) + 
                std::pow(p2.y - p1.y, 2) + 
                std::pow(p2.z - p1.z, 2));
            
            if (distance > gridSize_) {
                int nPoints = static_cast<int>(distance / gridSize_);
                for (int j = 0; j <= nPoints; ++j) {
                    double t = static_cast<double>(j) / nPoints;
                    densePoints.emplace_back(
                        p1.x + t * (p2.x - p1.x),
                        p1.y + t * (p2.y - p1.y),
                        p1.z + t * (p2.z - p1.z),
                        0
                    );
                }
            } else {
                densePoints.push_back(p1);
            }
        }
        densePoints.push_back(points.back());
    }
    
    return densePoints;
}

bool ImplicitBuilder::detectPlane(
    const std::vector<Point3D>& points, 
    char& planeType,
    double& value) {
    
    // 计算点集的协方差矩阵
    Eigen::Matrix3d covariance = Eigen::Matrix3d::Zero();
    Eigen::Vector3d mean = Eigen::Vector3d::Zero();
    
    // 计算均值
    for (const auto& p : points) {
        mean += Eigen::Vector3d(p.x, p.y, p.z);
    }
    mean /= points.size();
    
    // 计算协方差矩阵
    for (const auto& p : points) {
        Eigen::Vector3d v(p.x - mean[0], p.y - mean[1], p.z - mean[2]);
        covariance += v * v.transpose();
    }
    covariance /= points.size();
    
    // 特征值分解
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(covariance);
    Eigen::Vector3d eigenvalues = solver.eigenvalues();
    
    // 如果最小特征值接近0，说明是平面
    const double threshold = 1e-6;
    if (eigenvalues[0] < threshold * eigenvalues[2]) {
        // 找到最小特征值对应的方向
        Eigen::Vector3d normal = solver.eigenvectors().col(0);
        
        // 判断是否平行于坐标轴
        for (int i = 0; i < 3; ++i) {
            if (std::abs(std::abs(normal[i]) - 1.0) < threshold) {
                planeType = "xyz"[i];
                value = mean[i];
                return true;
            }
        }
    }
    
    return false;
}

bool ImplicitBuilder::detectFaultPlane(
    const std::vector<Point3D>& points,
    Point3D& normal,
    double& d) {
    
    // 构建点集矩阵
    Eigen::MatrixXd A(points.size(), 3);
    for (size_t i = 0; i < points.size(); ++i) {
        A(i, 0) = points[i].x;
        A(i, 1) = points[i].y;
        A(i, 2) = points[i].z;
    }
    
    // 计算中心点
    Eigen::Vector3d centroid = A.colwise().mean();
    A.rowwise() -= centroid.transpose();
    
    // SVD分解
    Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullV);
    Eigen::Vector3d n = svd.matrixV().col(2);
    
    // 计算奇异值比例
    const auto& s = svd.singularValues();
    double sRatio = s[1] / s[0];
    
    debug_log("奇异值: " + std::to_string(s[0]) + ", " + std::to_string(s[1]) + ", " + std::to_string(s[2]));
    debug_log("奇异值比例: ", sRatio);
    
    if (sRatio < 0.1) {  // 点集主要分布在一个平面上
        normal = Point3D(n[0], n[1], n[2], 0);
        d = -n.dot(centroid);
        return true;
    }
    
    return false;
}

bool ImplicitBuilder::validateSurface(
    const ImplicitSurface& surface,
    const std::vector<Point3D>& points,
    bool isFault,
    const std::string& faultName) {
    
    debug_log(isFault ? "断层界面验证结果" : "地层界面验证结果");
    if (!faultName.empty()) {
        debug_log("断层名称: " + faultName);
    }
    debug_log("界面点数量: ", points.size());
    
    // 验证界面点上的phi值
    double maxError = 0.0;
    double avgError = 0.0;
    
    for (const auto& p : points) {
        double phiVal = surface.evaluatePhi(p);
        maxError = std::max(maxError, std::abs(phiVal));
        avgError += std::abs(phiVal);
    }
    avgError /= points.size();
    
    debug_log("phi值最大误差: ", maxError);
    debug_log("phi值平均误差: ", avgError);
    
    // 输出样本点的值
    size_t sampleSize = std::min<size_t>(10, points.size());
    std::vector<size_t> sampleIndices(points.size());
    std::iota(sampleIndices.begin(), sampleIndices.end(), 0);
    std::random_device rd;
    std::mt19937 gen(rd());
    std::shuffle(sampleIndices.begin(), sampleIndices.end(), gen);
    
    debug_log("样本点phi值:");
    for (size_t i = 0; i < sampleSize; ++i) {
        const auto& p = points[sampleIndices[i]];
        double phiVal = surface.evaluatePhi(p);
        debug_log("点(" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "), phi值: " + std::to_string(phiVal));
    }
    
    // 验证界面两侧的点
    // ... (后续实现)
    
    return true;
}

std::variant<std::shared_ptr<ImplicitSurface>, std::vector<std::shared_ptr<ImplicitSurface>>>
ImplicitBuilder::buildFaultSurface(const std::vector<FaultNode>& faultNodes) {
    // 检查是否为稀疏情况
    if (isSparseCase(faultNodes)) {
        return buildSparseFaultSurface(faultNodes);
    }
    
    // 转换为Point3D数组
    std::vector<Point3D> points;
    points.reserve(faultNodes.size());
    for (const auto& node : faultNodes) {
        points.emplace_back(node.x, node.y, node.z, 0);
    }
    
    // 计算法向量
    auto normals = computeNormals(points, false);  // 不强制朝上
    
    // 生成训练点
    std::vector<Point3D> samplePoints;
    samplePoints.reserve(points.size() * 3);  // 界面点 + 正负偏移点
    
    // 界面点
    for (const auto& p : points) {
        samplePoints.push_back(p);
    }
    
    // 偏移点
    double offset = gridSize_ * 0.5;  // 使用较小的偏移距离
    for (size_t i = 0; i < points.size(); ++i) {
        const auto& p = points[i];
        const auto& n = normals[i];
        
        // 正面点
        samplePoints.emplace_back(
            p.x + n[0] * offset,
            p.y + n[1] * offset,
            p.z + n[2] * offset,
            0
        );
        
        // 负面点
        samplePoints.emplace_back(
            p.x - n[0] * offset,
            p.y - n[1] * offset,
            p.z - n[2] * offset,
            0
        );
    }
    
    // 构建RBF插值器，使用更小的epsilon值以获得更陡峭的过渡
    auto phi = [points, normals, this](const Point3D& p) -> double {
        // 找到最近的点
        std::vector<size_t> indices;
        std::vector<double> distances;
        auto kdtree = buildKDTree(points);
        kdtree->kNearest(Eigen::Vector3d(p.x, p.y, p.z), 1, indices, distances);
        
        const auto& nearestPoint = points[indices[0]];
        const auto& nearestNormal = normals[indices[0]];
        
        // 计算有向距离，使用更大的系数使得两侧符号更明显
        Eigen::Vector3d v(
            p.x - nearestPoint.x,
            p.y - nearestPoint.y,
            p.z - nearestPoint.z
        );
        
        // 使用更大的系数放大符号差异
        return 2.0 * v.dot(nearestNormal);
    };
    
    // 构建psi函数（断层有效范围）
    auto psi = buildFaultBoundaryFunction(points);
    
    // 计算包围盒
    Point3D boundsMin(std::numeric_limits<double>::max());
    Point3D boundsMax(std::numeric_limits<double>::lowest());
    for (const auto& p : points) {
        boundsMin.x = std::min(boundsMin.x, p.x - gridSize_);
        boundsMin.y = std::min(boundsMin.y, p.y - gridSize_);
        boundsMin.z = std::min(boundsMin.z, p.z - gridSize_);
        boundsMax.x = std::max(boundsMax.x, p.x + gridSize_);
        boundsMax.y = std::max(boundsMax.y, p.y + gridSize_);
        boundsMax.z = std::max(boundsMax.z, p.z + gridSize_);
    }
    
    return std::make_shared<ImplicitSurface>(
        phi, psi, ImplicitSurface::BoundingBox(boundsMin, boundsMax));
}

std::shared_ptr<ImplicitSurface> ImplicitBuilder::buildStrataSurface(
    const std::vector<Point3D>& points) {
    
    debug_log("开始构建地层界面，点数量: " + std::to_string(points.size()));
    
    // 先检查是否为平面
    char planeType;
    double planeValue;
    if (detectPlane(points, planeType, planeValue)) {
        debug_log("使用特殊平面处理: " + std::string(1, planeType));
        
        auto phi = [planeType, planeValue](const Point3D& p) -> double {
            switch (planeType) {
                case 'x': return p.x - planeValue;
                case 'y': return p.y - planeValue;
                case 'z': return p.z - planeValue;
                default: return 0.0;
            }
        };
        
        // 计算包围盒
        Point3D boundsMin = points[0];
        Point3D boundsMax = points[0];
        
        for (const auto& p : points) {
            boundsMin.x = std::min(boundsMin.x, p.x - gridSize_);
            boundsMin.y = std::min(boundsMin.y, p.y - gridSize_);
            boundsMin.z = std::min(boundsMin.z, p.z - gridSize_);
            boundsMax.x = std::max(boundsMax.x, p.x + gridSize_);
            boundsMax.y = std::max(boundsMax.y, p.y + gridSize_);
            boundsMax.z = std::max(boundsMax.z, p.z + gridSize_);
        }
        
        auto surface = std::make_shared<ImplicitSurface>(
            phi, nullptr, ImplicitSurface::BoundingBox(boundsMin, boundsMax));
            
        // 验证界面
        validateSurface(*surface, points);
        return surface;
    }
    
    // 计算法向量
    auto normals = computeNormals(points, true);  // 确保法向量朝上
    
    // 构建隐式函数
    auto phi = buildRBFInterpolator(points, normals);
    
    // 计算包围盒
    Point3D boundsMin(std::numeric_limits<double>::max());
    Point3D boundsMax(std::numeric_limits<double>::lowest());
    for (const auto& p : points) {
        boundsMin.x = std::min(boundsMin.x, p.x - gridSize_);
        boundsMin.y = std::min(boundsMin.y, p.y - gridSize_);
        boundsMin.z = std::min(boundsMin.z, p.z - gridSize_);
        boundsMax.x = std::max(boundsMax.x, p.x + gridSize_);
        boundsMax.y = std::max(boundsMax.y, p.y + gridSize_);
        boundsMax.z = std::max(boundsMax.z, p.z + gridSize_);
    }
    
    auto surface = std::make_shared<ImplicitSurface>(
        phi, nullptr, ImplicitSurface::BoundingBox(boundsMin, boundsMax));
    
    // 验证界面
    validateSurface(*surface, points);
    
    // 额外验证：检查界面两侧的点
    debug_log("\n检查界面两侧的点:");
    for (size_t i = 0; i < points.size(); ++i) {
        const auto& p = points[i];
        const auto& n = normals[i];
        
        // 检查上下两侧的点
        double upDist = gridSize_;
        double downDist = -gridSize_;
        
        Point3D upPoint(
            p.x + n[0] * upDist,
            p.y + n[1] * upDist,
            p.z + n[2] * upDist
        );
        
        Point3D downPoint(
            p.x + n[0] * downDist,
            p.y + n[1] * downDist,
            p.z + n[2] * downDist
        );
        
        double upValue = surface->evaluatePhi(upPoint);
        double downValue = surface->evaluatePhi(downPoint);
        
        if (upValue < 0 || downValue > 0) {
            debug_log("警告: 点 " + std::to_string(i) + " 的上下值异常");
            debug_log("上方点值: " + std::to_string(upValue));
            debug_log("下方点值: " + std::to_string(downValue));
        }
    }
    
    return surface;
}

// 添加私有辅助方法

void ImplicitBuilder::findKNearestNeighbors(
    const Eigen::MatrixXd& points,
    int k,
    Eigen::MatrixXi& indices,
    Eigen::MatrixXd& distances) const {
    
    // 构建KD树
    KDTree kdtree(points);
    
    // 对每个点查找k个最近邻
    const int n = points.rows();
    indices.resize(n, k);
    distances.resize(n, k);
    
    for (int i = 0; i < n; ++i) {
        std::vector<size_t> nn_indices;
        std::vector<double> nn_distances;
        kdtree.kNearest(points.row(i), k + 1, nn_indices, nn_distances);
        
        // 跳过第一个点（是查询点自身）
        for (int j = 0; j < k; ++j) {
            indices(i, j) = nn_indices[j + 1];
            distances(i, j) = std::sqrt(nn_distances[j + 1]);
        }
    }
}

ImplicitSurface::ImplicitFunction ImplicitBuilder::buildRBFInterpolator(
    const std::vector<Point3D>& points,
    const std::vector<Eigen::Vector3d>& normals) {
    
    // 构建KD树
    auto kdtree = buildKDTree(points);
    
    // 返回插值函数
    return [points, normals, kdtree](const Point3D& p) -> double {
        // 找到最近的点
        std::vector<size_t> indices;
        std::vector<double> distances;
        kdtree->kNearest(Eigen::Vector3d(p.x, p.y, p.z), 1, indices, distances);
        
        const auto& nearestPoint = points[indices[0]];
        const auto& nearestNormal = normals[indices[0] % normals.size()];  // 使用模运算处理偏移点
        
        // 计算有向距离
        Eigen::Vector3d v(
            p.x - nearestPoint.x,
            p.y - nearestPoint.y,
            p.z - nearestPoint.z
        );
        
        return v.dot(nearestNormal);
    };
}

double ImplicitBuilder::rbfKernel(double r, double epsilon) const {
    double er = epsilon * r;
    if (er > 3.0) return 0.0;  // 更紧凑的支撑
    return std::exp(-er * er);  // 高斯核
}

std::function<double(const Point3D&)> ImplicitBuilder::buildFaultBoundaryFunction(
    const std::vector<Point3D>& points) {
    
    // 提取xy平面上的点
    std::vector<Eigen::Vector2d> xy_points;
    xy_points.reserve(points.size());
    for (const auto& p : points) {
        xy_points.emplace_back(p.x, p.y);
    }
    
    // 计算凸包
    std::vector<Eigen::Vector2d> hull;
    try {
        // Graham扫描法计算凸包
        auto grahamScan = [](std::vector<Eigen::Vector2d>& points) {
            if (points.size() < 3) return points;
            
            // 找到最下方的点作为起点
            auto start = std::min_element(points.begin(), points.end(),
                [](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
                    return a.y() < b.y() || (a.y() == b.y() && a.x() < b.x());
                });
            std::swap(*points.begin(), *start);
            
            // 按极角排序
            Eigen::Vector2d p0 = points[0];
            std::sort(points.begin() + 1, points.end(),
                [&p0](const Eigen::Vector2d& a, const Eigen::Vector2d& b) {
                    double cross = (a - p0).x() * (b - p0).y() - 
                                 (a - p0).y() * (b - p0).x();
                    if (std::abs(cross) < 1e-10)
                        return (a - p0).squaredNorm() < (b - p0).squaredNorm();
                    return cross > 0;
                });
            
            // Graham扫描
            std::vector<Eigen::Vector2d> hull;
            hull.push_back(points[0]);
            hull.push_back(points[1]);
            
            for (size_t i = 2; i < points.size(); ++i) {
                while (hull.size() >= 2) {
                    Eigen::Vector2d v1 = hull.back() - hull[hull.size()-2];
                    Eigen::Vector2d v2 = points[i] - hull.back();
                    if (v1.x() * v2.y() - v1.y() * v2.x() > 0)
                        break;
                    hull.pop_back();
                }
                hull.push_back(points[i]);
            }
            
            return hull;
        };
        
        hull = grahamScan(xy_points);
    }
    catch (const std::exception& e) {
        debug_log("计算凸包失败: " + std::string(e.what()));
        return [](const Point3D&) { return 0.0; };
    }
    
    // 返回边界函数
    return [hull, this](const Point3D& p) -> double {
        Eigen::Vector2d point(p.x, p.y);
        
        // 计算点到凸包边界的最小距离
        double minDist = std::numeric_limits<double>::max();
        for (size_t i = 0; i < hull.size(); ++i) {
            const auto& p1 = hull[i];
            const auto& p2 = hull[(i + 1) % hull.size()];
            double dist = pointToSegmentDistance(point, p1, p2);
            minDist = std::min(minDist, dist);
        }
        
        // 判断点是否在凸包内
        bool inside = isPointInPolygon(point, hull);
        
        return inside ? -minDist : minDist;
    };
}

double ImplicitBuilder::pointToSegmentDistance(
    const Eigen::Vector2d& point,
    const Eigen::Vector2d& segStart,
    const Eigen::Vector2d& segEnd) const {
    
    Eigen::Vector2d v = segEnd - segStart;
    Eigen::Vector2d w = point - segStart;
    
    double c1 = w.dot(v);
    if (c1 <= 0)
        return (point - segStart).norm();
        
    double c2 = v.dot(v);
    if (c2 <= c1)
        return (point - segEnd).norm();
        
    double b = c1 / c2;
    Eigen::Vector2d pb = segStart + b * v;
    return (point - pb).norm();
}

bool ImplicitBuilder::isPointInPolygon(
    const Eigen::Vector2d& point,
    const std::vector<Eigen::Vector2d>& polygon) const {
    
    bool inside = false;
    for (size_t i = 0, j = polygon.size() - 1; i < polygon.size(); j = i++) {
        if (((polygon[i].y() > point.y()) != (polygon[j].y() > point.y())) &&
            (point.x() < (polygon[j].x() - polygon[i].x()) * 
             (point.y() - polygon[i].y()) / (polygon[j].y() - polygon[i].y()) +
             polygon[i].x())) {
            inside = !inside;
        }
    }
    return inside;
}

// 添加computeNormals的实现
std::vector<Eigen::Vector3d> ImplicitBuilder::computeNormals(
    const std::vector<Point3D>& points,
    bool ensureUpward) const {
    
    // 构建点集矩阵
    Eigen::MatrixXd pointMatrix(points.size(), 3);
    for (size_t i = 0; i < points.size(); ++i) {
        pointMatrix.row(i) = Eigen::Vector3d(points[i].x, points[i].y, points[i].z);
    }
    
    // 计算每个点的法向量
    std::vector<Eigen::Vector3d> normals;
    normals.reserve(points.size());
    
    const int k = std::min(20, static_cast<int>(points.size()));
    
    // 构建KD树
    KDTree kdtree(pointMatrix);
    
    for (size_t i = 0; i < points.size(); ++i) {
        // 找到最近邻点
        std::vector<size_t> indices;
        std::vector<double> distances;
        kdtree.kNearest(pointMatrix.row(i), k, indices, distances);
        
        // 构建局部点集
        Eigen::MatrixXd neighbors(k, 3);
        for (int j = 0; j < k; ++j) {
            neighbors.row(j) = pointMatrix.row(indices[j]);
        }
        
        // 计算局部PCA
        Eigen::Vector3d centroid = neighbors.colwise().mean();
        neighbors.rowwise() -= centroid.transpose();
        
        Eigen::JacobiSVD<Eigen::MatrixXd> svd(neighbors, Eigen::ComputeFullV);
        Eigen::Vector3d normal = svd.matrixV().col(2);
        
        // 确保法向量朝上
        if (ensureUpward && normal[2] < 0) {
            normal = -normal;
        }
        
        normals.push_back(normal);
    }
    
    return normals;
}

std::shared_ptr<KDTree> ImplicitBuilder::buildKDTree(
    const std::vector<Point3D>& points) const {
    
    Eigen::MatrixXd pointMatrix(points.size(), 3);
    for (size_t i = 0; i < points.size(); ++i) {
        pointMatrix.row(i) = Eigen::Vector3d(points[i].x, points[i].y, points[i].z);
    }
    
    return std::make_shared<KDTree>(pointMatrix);
}

bool ImplicitBuilder::isSparseCase(const std::vector<FaultNode>& faultNodes) const {
    // 统计柱线数量和每个柱线上的点数
    std::map<int, int> pillarPoints;
    for (const auto& node : faultNodes) {
        pillarPoints[node.column_id]++;
    }
    
    // 检查条件：柱线数量<=3且每个柱线上的点数<=3
    if (pillarPoints.size() > 3) return false;
    
    for (const auto& [id, count] : pillarPoints) {
        if (count > 3) return false;
    }
    
    return true;
}

std::vector<std::vector<FaultNode>> ImplicitBuilder::groupNodesByPillar(
    const std::vector<FaultNode>& faultNodes) const {
    
    // 按柱线ID分组并按z坐标排序
    std::map<int, std::vector<FaultNode>> pillarGroups;
    for (const auto& node : faultNodes) {
        pillarGroups[node.column_id].push_back(node);
    }
    
    std::vector<std::vector<FaultNode>> result;
    for (auto& [id, nodes] : pillarGroups) {
        std::sort(nodes.begin(), nodes.end(),
            [](const FaultNode& a, const FaultNode& b) { return a.z < b.z; });
        result.push_back(nodes);
    }
    
    return result;
}

std::vector<std::pair<std::vector<Point3D>, std::vector<Point3D>>> 
ImplicitBuilder::generatePlanePairs(
    const std::vector<std::vector<FaultNode>>& pillars) const {
    
    std::vector<std::pair<std::vector<Point3D>, std::vector<Point3D>>> planePairs;
    
    // 对每对相邻柱线生成平面对
    for (size_t i = 0; i < pillars.size() - 1; ++i) {
        const auto& pillar1 = pillars[i];
        const auto& pillar2 = pillars[i + 1];
        
        // 确保两个柱线有相同数量的点
        size_t numPoints = std::min(pillar1.size(), pillar2.size());
        
        // 生成平面对
        for (size_t j = 0; j < numPoints - 1; ++j) {
            std::vector<Point3D> plane1 = {
                {pillar1[j].x, pillar1[j].y, pillar1[j].z, 0},
                {pillar1[j+1].x, pillar1[j+1].y, pillar1[j+1].z, 0},
                {pillar2[j].x, pillar2[j].y, pillar2[j].z, 0},
                {pillar2[j+1].x, pillar2[j+1].y, pillar2[j+1].z, 0}
            };
            
            // 为下一段准备点
            std::vector<Point3D> plane2;
            if (j < numPoints - 2) {
                plane2 = {
                    {pillar1[j+1].x, pillar1[j+1].y, pillar1[j+1].z, 0},
                    {pillar1[j+2].x, pillar1[j+2].y, pillar1[j+2].z, 0},
                    {pillar2[j+1].x, pillar2[j+1].y, pillar2[j+1].z, 0},
                    {pillar2[j+2].x, pillar2[j+2].y, pillar2[j+2].z, 0}
                };
            }
            
            planePairs.push_back({plane1, plane2});
        }
    }
    
    return planePairs;
}

std::vector<std::shared_ptr<ImplicitSurface>> 
ImplicitBuilder::buildSparseFaultSurface(const std::vector<FaultNode>& faultNodes) {
    // 1. 按柱线分组并排序
    auto pillars = groupNodesByPillar(faultNodes);
    
    // 2. 生成平面对
    auto planePairs = generatePlanePairs(pillars);
    
    // 3. 为每个平面构建独立的隐式函数
    std::vector<std::shared_ptr<ImplicitSurface>> surfaces;
    for (const auto& [plane1, plane2] : planePairs) {
        // 计算平面1的法向量和距离
        Eigen::Vector3d v1(plane1[1].x - plane1[0].x,
                          plane1[1].y - plane1[0].y,
                          plane1[1].z - plane1[0].z);
        Eigen::Vector3d v2(plane1[2].x - plane1[0].x,
                          plane1[2].y - plane1[0].y,
                          plane1[2].z - plane1[0].z);
        Eigen::Vector3d normal1 = v1.cross(v2).normalized();
        double d1 = -normal1.dot(Eigen::Vector3d(plane1[0].x, plane1[0].y, plane1[0].z));
        
        // 构建隐式函数
        auto phi = [normal1, d1](const Point3D& p) -> double {
            Eigen::Vector3d point(p.x, p.y, p.z);
            return normal1.dot(point) + d1;
        };
        
        // 计算包围盒
        Point3D boundsMin = plane1[0], boundsMax = plane1[0];
        for (const auto& p : plane1) {
            boundsMin.x = std::min(boundsMin.x, p.x);
            boundsMin.y = std::min(boundsMin.y, p.y);
            boundsMin.z = std::min(boundsMin.z, p.z);
            boundsMax.x = std::max(boundsMax.x, p.x);
            boundsMax.y = std::max(boundsMax.y, p.y);
            boundsMax.z = std::max(boundsMax.z, p.z);
        }
        
        // 扩大包围盒以确保覆盖
        double margin = gridSize_ * 0.1;
        boundsMin.x -= margin; boundsMin.y -= margin; boundsMin.z -= margin;
        boundsMax.x += margin; boundsMax.y += margin; boundsMax.z += margin;
        
        surfaces.push_back(std::make_shared<ImplicitSurface>(
            phi, nullptr, std::make_pair(boundsMin, boundsMax)));
    }
    
    return surfaces;
}

// ... (其他辅助方法的实现) 